function Err_C=Contstraint_Err(Err_p)
if(norm(Err_p(1:3))>10)   
        Err_p(1:3)=Err_p(1:3)/2;
end
if(norm(Err_p(1:3))>5)   
        Err_p(1:3)=Err_p(1:3)/1.5;
end
if(norm(Err_p(1:3))>3)   
        Err_p(1:3)=Err_p(1:3)/1.2;
end
 if(norm(Err_p(1:3))>1)   
        Err_p(1:3)=Err_p(1:3);
 end
 Err_C=Err_p;

